% Code for Fig2
clc
clear all
% close all

A = [0.5 -0.3;
     -0.8 0.1];
A5 = [-0.11 0.03;
      0.17 -0.11];
D = [-0.2;0.1];

nx = size(A,1);
nd = size(D,2);

H0 = 1*eye(nx); 
Hw = 0.1*eye(nd);

Phi = zeros(nx,nx,6);
Phi(:,:,1) = eye(nx);
Hwk = zeros(nx,nx);
Hwk1 = zeros(nx,nx);
Hwk2 = zeros(nx,nx);
Hwk3 = zeros(nx,nx);
Hwk4 = zeros(nx,nx);
Hwk5 = zeros(nx,nx);

Phi_k = eye(nx);
Phi_k1 = zeros(nx,nx);
Phi_k2 = zeros(nx,nx);
Phi_k3 = zeros(nx,nx);
Phi_k4 = zeros(nx,nx);
Phi_k5 = zeros(nx,nx);

AA = zeros(nx,nx,6);
AA(:,:,1) = A;
AA(:,:,6) = A5;

Nk = 15;

vk = zeros(1,Nk);
Hk = eye(nx);
Xk = zeros(nx,1)+Hk*unitbox(nx,1);
vk(:,1) = volume(Xk);

HHk = [zeros(1,nx);eye(nx)];
ck = [1;0;0];
XXk = ck+HHk*unitbox(nx,1);        
figure(1)
hold on
Options.color='b';
Options.alpha=0.2;
plot(XXk,Options)
    
for k=2:Nk  
    Phi_k_tmp = AA(:,:,1)*Phi_k+AA(:,:,2)*Phi_k1+AA(:,:,3)*Phi_k2+AA(:,:,4)*Phi_k3+ ... 
        AA(:,:,5)*Phi_k4+AA(:,:,6)*Phi_k5;
    Phi_k5 = Phi_k4;
    Phi_k4 = Phi_k3;
    Phi_k3 = Phi_k2;
    Phi_k2 = Phi_k1;
    Phi_k1 = Phi_k;
    Phi_k = Phi_k_tmp;
  
    Hwk_tmp = [AA(:,:,1)*Hwk AA(:,:,2)*Hwk1 AA(:,:,3)*Hwk2 AA(:,:,4)*Hwk3 ... 
        AA(:,:,5)*Hwk4 AA(:,:,6)*Hwk5 D*Hw];
    Hwk5 = Hwk4;
    Hwk4 = Hwk3;
    Hwk3 = Hwk2;
    Hwk2 = Hwk1;
    Hwk1 = Hwk;
    Hwk = Hwk_tmp;
    
    Hwk = reduceG(Hwk,10);
    Hk = [Phi_k*H0 Hwk];
    s = size(Hk,2);
    Xk = zeros(nx,1)+Hk*unitbox(s,1);     
    vk(:,k) = volume(Xk);
    
    HHk = [zeros(1,s);Hk]
    ck = [k;0;0];
    XXk = ck+HHk*unitbox(s,1);        
    figure(1)
    hold on
    Options.color='b';
    Options.alpha=0.2;
    plot(XXk,Options)
end

figure(2)
hold on
plot(1:Nk,vk,'b>')
